﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Hook.Graphics;
using Hook.Environmental.Props;
using Hook.Environmental.Effects;
using Microsoft.Xna.Framework.Input;
using Hook.Primitives;
using Hook.Environmental.Props.EnvironmentPieces;
using Hook.Environmental.Levels;
using Hook.Environmental.Props.Enemies;
using Microsoft.Xna.Framework.Media;

namespace Hook.Environmental
{
    /// <summary>
    /// Environment generator and resolver. Contains all the game props to be handled, 
    /// as well as collision and input.
    /// </summary>
    public class GameEnvironment
    {
        public const float MAXVEL = 25.0f;
        public const float MAXANGLEVEL = 0.2f;
        public const float ELASTICITY = 1;
        public static Vector3 GRAVITY = new Vector3(0, 0.5f, 0);

        public List<Level> Levels;
        public int CurrentLevelNum;

        public Camera Cam;
        public List<Prop> Props;
        public List<GameEffect> Effects;
        public Player Player;
        public Prop CurrentLevelStart, CurrentLevelFinish;
        //public List<Prop>[,] Propss;
        //public int PropTileR, PropTileC;
        public List<HingeLink> HingeLinks;
        public Vector3 MousePosition;

        ChainHead ChainHead;
        //DieThrower DieThrower;
        //Prop A, B, C;
        //Vector3 HingeRA, HingeRB, HingeRBC, HingeRCB;
        //HingeChain TestChain;
        StretchChain StretchChain;
        Vector3 StretchForce;

        float UpdateDistance, PlayerVel;
        Vector3 HingePositionA, HingePositionB;

        int MouseWheelPrevious, MouseWheelCurrent;
        bool LeftMouseDown, RightMouseDown, KeySpaceDown, LevelLoaded;
        Vector3 LocationOffset, ForceVector;

        SnapTrap TestTrap;
        Bird TestBird;
        Panther TestPanther;

        Prop TestPlatform;

        public GameEnvironment()
        {
            CurrentLevelNum = 0;
            Levels = new List<Level>();
            Levels.Add(new Level0());
            Levels.Add(new Level("Forest.txt"));

            Cam = new Camera();
            Props = new List<Prop>();
            Effects = new List<GameEffect>();
            //Propss = new List<Prop>[2,4];
            //Propss = new List<Prop>[Levels[CurrentLevelNum].NumberOfRows,Levels[CurrentLevelNum].NumberOfColumns];
            HingeLinks = new List<HingeLink>();

            MousePosition = Vector3.Zero;
            MouseWheelCurrent = MouseWheelPrevious = Mouse.GetState().ScrollWheelValue;

            LevelLoaded = false;

            //InitializeEnvironment();
        }
        
        public void InitializeEnvironment()
        {
            Props.Clear();

            StretchChain = new StretchChain();

            ChainHead = new ChainHead();
            Props.Add(ChainHead);
            StretchChain.Props.Add(ChainHead);

            Player = new Player();
            Props.Add(Player);
            StretchChain.Props.Add(Player);

            LoadLevel(CurrentLevelNum);

            UpdateDistance = PlayerVel = 0;
            HingePositionA = HingePositionB = Vector3.Zero;

            LocationOffset = ForceVector = Vector3.Zero;
            LevelLoaded = true;
        }

        /// <summary>
        /// Load specified level objects.
        /// </summary>
        public void LoadLevel(int LevelIndex)
        {
            //Props.Clear();

            Props.AddRange(Levels[LevelIndex].Props);
            CurrentLevelStart = Levels[LevelIndex].LevelStart;
            Props.Add(CurrentLevelStart);
            CurrentLevelFinish = Levels[LevelIndex].LevelFinish;
            Props.Add(CurrentLevelFinish);
            Player.SetPosition(CurrentLevelStart.Position);

            MediaPlayer.Play(Game1.Music[Levels[LevelIndex].LevelMusic]);
            MediaPlayer.IsRepeating = true;
        }
        
        /// <summary>
        /// Modify game variables based on input.
        /// </summary>
        public void HandleInput()
        {
            KeyboardState kbs = Keyboard.GetState();
            MouseState ms = Mouse.GetState();

            MousePosition = new Vector3(ms.X - Game1.RESOLUTION.X / 2, ms.Y - Game1.RESOLUTION.Y / 2, 0) + Cam.Position;
            MouseWheelCurrent = ms.ScrollWheelValue;

            if (LevelLoaded)
            {
                if (kbs.IsKeyDown(Keys.D))
                {
                    if (StretchChain.ChainState == StretchChain.STICK)
                    {
                        Player.QueueForce(Player.Mass * new Vector3(.3f, 0, 0));
                    }
                    else
                    {
                        Player.MoveRight();
                    }
                }
                if (kbs.IsKeyDown(Keys.S))
                {
                    if (StretchChain.ChainState == StretchChain.STICK)
                    {
                        Player.QueueForce(Player.Mass * new Vector3(0, .3f, 0));
                    }
                }
                if (kbs.IsKeyDown(Keys.A))
                {
                    if (StretchChain.ChainState == StretchChain.STICK)
                    {
                        Player.QueueForce(Player.Mass * new Vector3(-.3f, 0, 0));
                    }
                    else
                    {
                        Player.MoveLeft();
                    }
                }
                if (kbs.IsKeyDown(Keys.W))
                {
                    if (StretchChain.ChainState == StretchChain.STICK)
                    {
                        Player.QueueForce(Player.Mass * new Vector3(0, -.3f, 0));
                    }
                }

                if (kbs.IsKeyUp(Keys.D) && kbs.IsKeyUp(Keys.A))
                {
                    Player.Coast();
                }

                if (kbs.IsKeyDown(Keys.PageDown))
                {
                    Player.QueueTorque(new Vector3(0, 0, (float)Math.PI / 2));
                }
                if (kbs.IsKeyDown(Keys.PageUp))
                {
                    Player.QueueTorque(new Vector3(0, 0, (float)Math.PI / -2));
                }

                if (kbs.IsKeyDown(Keys.Space) && !KeySpaceDown)
                {
                    //Player.Velocity.Y = 0;
                    //Player.QueueForce(new Vector3(0, -20 * Player.Mass, 0));
                    if (Player.CanJump())
                    {
                        Player.Jump();
                    }
                    KeySpaceDown = true;
                }
                if (kbs.IsKeyUp(Keys.Space))
                {
                    KeySpaceDown = false;
                }

                if (kbs.IsKeyDown(Keys.NumPad4))
                {
                    Cam.Position += new Vector3(-5, 0, 0);
                }
                if (kbs.IsKeyDown(Keys.NumPad6))
                {
                    Cam.Position += new Vector3(5, 0, 0);
                }
                if (kbs.IsKeyDown(Keys.NumPad8))
                {
                    Cam.Position += new Vector3(0, -5, 0);
                }
                if (kbs.IsKeyDown(Keys.NumPad2))
                {
                    Cam.Position += new Vector3(0, 5, 0);
                }
                if (kbs.IsKeyDown(Keys.Add))
                {
                    Cam.Zoom += .025f;
                }
                if (kbs.IsKeyDown(Keys.Subtract))
                {
                    Cam.Zoom -= .025f;
                }

                if (kbs.IsKeyDown(Keys.LeftControl))
                {
                    Effects.Add(new PlayerParticleEffect(Player.Position));
                }
                if (kbs.IsKeyDown(Keys.LeftShift))
                {
                    Effects.Clear();
                    KillPlayer();
                }
            }

            if (kbs.IsKeyDown(Keys.D0))
            {
                CurrentLevelNum = 0;
            }
            if (kbs.IsKeyDown(Keys.D1))
            {
                CurrentLevelNum = 1;
            }

            if (ms.LeftButton == ButtonState.Pressed && !LeftMouseDown)
            {
                if (StretchChain.CanExtend())
                {
                    StretchChain.Extend();

                    List<Prop>SJs = new List<Prop>();
                    for (int j = 0; j < Player.BankedStretchJoints; j++)
                    {
                        //StretchJoint SJ = new StretchJoint();
                        Prop SJ = new Prop("StretchJoint.txt");
                        SJ.Position = Player.Position;
                        SJs.Add(SJ);
                        Props.Add(SJ);
                    }
                    StretchChain.AddJoints(SJs);
                    Player.BankedStretchJoints = 0;
                    StretchChain.PositionChain(Player.Position);

                    Vector3 DirectionVector = MousePosition - ChainHead.Position;
                    DirectionVector.Normalize();
                    ChainHead.Velocity = Vector3.Zero;
                    ChainHead.QueueForce(15 * ChainHead.Mass * DirectionVector);
                }

                //LocationOffset = new Vector3(ms.X - Game1.RESOLUTION.X / 2, ms.Y - Game1.RESOLUTION.Y / 2, 0) - Player.Position;
                LeftMouseDown = true;
            }
            if (ms.LeftButton == ButtonState.Released && LeftMouseDown)
            {
                //ForceVector = new Vector3(ms.X - Game1.RESOLUTION.X / 2, ms.Y - Game1.RESOLUTION.Y / 2, 0) - (Player.Position + LocationOffset);
                //Player.QueueForceWithOffset(ForceVector / 50, LocationOffset);
                LeftMouseDown = false;
            }
            if (ms.RightButton == ButtonState.Pressed && !RightMouseDown)
            {
                //TestChain.ReverseChainLinks();

                if (StretchChain.CanContract())
                {
                    //if (ChainHead.Rooted)
                        //ChainHead.UnRoot();

                    StretchChain.Contract();
                    //else
                        //ChainHead.Root();
                }

                //LocationOffset = new Vector3(ms.X - Game1.RESOLUTION.X / 2, ms.Y - Game1.RESOLUTION.Y / 2, 0) - Player.Position;
                RightMouseDown = true;
            }
            if (ms.RightButton == ButtonState.Released)
            {
                //ForceVector = new Vector3(ms.X - Game1.RESOLUTION.X / 2, ms.Y - Game1.RESOLUTION.Y / 2, 0) - (Player.Position + LocationOffset);
                //Player.QueueForceWithOffset(ForceVector / 50, LocationOffset);
                RightMouseDown = false;
            }

            int MouseScrollDifference = MouseWheelCurrent - MouseWheelPrevious;
            if (MouseScrollDifference > 0)
            {
                if (StretchChain.ChainState == StretchChain.STICK)
                {
                    Prop RemovedJoint = StretchChain.RemoveLastJoint();
                    if (RemovedJoint != null)
                    {
                        Props.Remove(RemovedJoint);
                        Player.BankedStretchJoints++;
                    }
                }
            }
            else if (MouseScrollDifference < 0)
            {
                if (StretchChain.ChainState == StretchChain.STICK)
                {
                    if (Player.BankedStretchJoints > 0)
                    {
                        //StretchJoint SJ = new StretchJoint();
                        Prop SJ = new Prop("StretchJoint.txt");
                        SJ.Position = Player.Position;
                        StretchChain.AddJoint(SJ);
                        Props.Add(SJ);
                        Player.BankedStretchJoints--;
                    }
                }
            }
            MouseWheelPrevious = MouseWheelCurrent;

            if (kbs.IsKeyDown(Keys.End))
            {
                InitializeEnvironment();
            }
        }

        public void RunGameLogic()
        {
            for (int p = 0; p < Props.Count; p++)
            {
                if (Props[p].Name.Equals("SnapTrap.txt"))
                {
                    if (Props[p].WaitTimer == 0 && (Props[p].Position - Player.Position).Length() < 150)
                    {
                        Props[p].WaitTimer = 3;
                        Prop P = new Prop("SnapTrapSnap.txt");
                        P.SetPosition(Props[p].Position + new Vector3(0, -100, 0));
                        P.RemovalTimer = 1.5;
                        Props.Add(P);
                    }
                }
                else if (Props[p].Name.Equals("Bird.txt"))
                {
                    if (Props[p].RootedTo)
                    {
                        Props[p].Velocity = new Vector3(0, -5, 0);
                    }
                    else
                    {
                        Props[p].Velocity = Vector3.Zero;
                    }
                }
            }
        }

        public void Update(GameTime gt)
        {
            HandleInput();

            if (LevelLoaded)
            {
                ApplyPropGravity();

                //EnforceHingeConstraints(TestChain);

                StretchChain.ApplySpringForce();
                StretchChain.UpdateVelocities();
                StretchChain.ApplySpringDamping();

                StretchForce = StretchChain.Props[1].QueuedForce;

                foreach (Prop P in Props)
                {
                    if ((P.Position - Player.Position).Length() < 2000)
                    {
                        P.Update(gt);
                    }
                }

                RunGameLogic();

                Player.RestrictVelocity();
                PlayerVel = Player.Velocity.X;

                foreach (Prop P in Props)
                {
                    if ((P.Position - Player.Position).Length() < 2000)
                    {
                        P.Move();
                    }
                }

                //TestChain.SetHingePositions();
                /*foreach (HingeLink HL in TestChain.HingeLinks)
                {
                    HL.AdjustHingePositions();
                }*/

                UnGroundProps();
                CheckPropCollision(Props);
                ChainHead.FollowRoot();

                CheckLevelFinish();

                for (int p = 0; p < Props.Count; p++)
                {
                    if (Props[p].Removing)
                    {
                        Props.RemoveAt(p);
                        p--;
                    }
                }

                foreach (GameEffect E in Effects)
                {
                    E.Update(gt);
                }
                for (int e = 0; e < Effects.Count; e++)
                {
                    if (Effects[e].Removing)
                    {
                        Effects.RemoveAt(e);
                        e--;
                    }
                }

                Cam.Position = Player.Position;
            }

            //HingePositionA = A.Position + Prop.RotateVectorAboutZ(HingeRA, A.Rotation.Z);
            //HingePositionB = B.Position + Prop.RotateVectorAboutZ(HingeRB, B.Rotation.Z);
            //UpdateDistance = (B.Position - A.Position).Length();
        }
        public void Draw(SpriteBatch sb)
        {
            /*foreach (HingeLink HL in HingeLinks)
            {
                HL.AdjustHingePositions();
            }*/
            //HingePositionA = A.Position + Prop.RotateVectorAboutZ(HingeRA, A.Rotation.Z);
            //HingePositionB = B.Position + Prop.RotateVectorAboutZ(HingeRB, B.Rotation.Z);
            //Vector3 HingePointA = A.Frame.Points[0].Position;
            //Vector3 HingePointB = B.Frame.Points[2].Position;
            //Vector3 HingeDistance = HingePointB - HingePointA;

            //DrawDistance = (B.Position - A.Position).Length();
            if (LevelLoaded)
            {
                for (int n = 0; n < 3; n++)
                {
                    foreach (Prop p in Props)
                    {
                        if (p.DrawPriority == n)
                            p.Draw(sb, Cam);
                    }

                    foreach (GameEffect E in Effects)
                    {
                        if (E.DrawPriority == n)
                            E.Draw(sb, Cam);
                    }
                }

                //sb.DrawString(Game1.GameFont, "HingeA: " + HingeDistance, new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 80), Color.Black);
                sb.DrawString(Game1.GameFont, "PXVel: " + Math.Round(PlayerVel, 1), new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 60), Color.Black);
                sb.DrawString(Game1.GameFont, "ChainState: " + StretchChain.ChainState, new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 40), Color.Black);
                sb.DrawString(Game1.GameFont, "Grounded: " + Player.Grounded, new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 20), Color.Black);
            }

            /*if (Props[0].AttachedProps.Count > 0)
            {
                //sb.DrawString(Game1.GameFont, "Prop Space: " + Props[0].AttachedProps[0].AttachedProps[0].Frame.OriginOffset.Length(), new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 60), Color.Black);
                sb.DrawString(Game1.GameFont, "Prop Pos: " + Props[0].AttachedProps[0].Position, new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 40), Color.Black);
                sb.DrawString(Game1.GameFont, "Frame Pos: " + (Props[0].AttachedProps[0].Frame.ReferencePoint.Position + Props[0].AttachedProps[0].Frame.OriginOffset), new Vector2(Game1.RESOLUTION.X / 2, Game1.RESOLUTION.Y - 20), Color.Black);
            }*/
        }

        public void CheckLevelFinish()
        {
            if(WithinPropRange(Player, CurrentLevelFinish))
            {
                Player.SetPosition(CurrentLevelStart.Position);
            }
        }

        public void KillPlayer()
        {
            Player.Removing = true;
            Effects.Add(new BloodFountain(Player.Position));
            Random R = new Random();
            for (int r = 0; r < 3; r++)
            {
                for (int c = 0; c < 2; c++)
                {
                    PlayerChunk PC = new PlayerChunk(Player.Position, r, c);
                    float XForce = 20 * (float)(R.NextDouble() - .5);
                    float YForce = 20 * (float)(R.NextDouble() - .5);
                    PC.QueueForce(new Vector3(XForce, YForce, 0));
                    Props.Add(PC);
                }
            }
        }

        /// <summary>
        /// Check if the distance between two props is close enough for collision.
        /// </summary>
        public bool WithinPropRange(Prop P1, Prop P2)
        {
            float PropRadius1, PropRadius2;
            if (P1.Frame.FarthestPointIndex == -1)
            {
                PropRadius1 = 0;
            }
            else
            {
                PropRadius1 = P1.Frame.Offsets[P1.Frame.FarthestPointIndex].Length();
            }
            if (P2.Frame.FarthestPointIndex == -1)
            {
                PropRadius2 = 0;
            }
            else
            {
                PropRadius2 = P2.Frame.Offsets[P2.Frame.FarthestPointIndex].Length();
            }
            float PropOriginDistance = (P2.Position - P1.Position).Length();
            if (PropRadius1 + PropRadius2 > PropOriginDistance)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// Start a collision check for each prop in the list.
        /// </summary>
        public void CheckPropCollision(List<Prop> PList)
        {
            for (int p1 = 0; p1 < PList.Count; p1++)
            {
                for (int p2 = 0; p2 < PList.Count; p2++)
                {
                    if(Props[p1] is ChainHead && Props[p2] is Player && StretchChain.ChainState == StretchChain.CONTRACT && WithinPropRange(Props[p1], Props[p2]))
                    {
                        List<Prop> RemovedJoints = StretchChain.RemoveAllJoints();
                        Player.BankedStretchJoints += RemovedJoints.Count;
                        foreach (Prop P in RemovedJoints)
                        {
                            Props.Remove(P);
                        }
                        StretchChain.Rest();
                    }
                    else if(Props[p1].Name.Equals("StretchJoint.txt") && Props[p2] is Player && StretchChain.ChainState == StretchChain.CONTRACT && WithinPropRange(Props[p1], Props[p2]))
                    {
                        StretchChain.Props.Remove(Props[p1]);
                        Player.BankedStretchJoints++;
                        Props[p1].Removing = true;
                    }

                    if (p1 != p2 && WithinPropRange(Props[p1], Props[p2]) && Prop.HasCommonLayer(Props[p1], Props[p2]) && !(Props[p1].Name.Equals("StretchJoint.txt") && Props[p2].Name.Equals("StretchJoint.txt")))
                    {
                        PolyPoint CollidaPoint = CheckPointCollision(Props[p1], Props[p2]);
                        if (CollidaPoint != null)
                            ResolvePropPointCollision(Props[p1], Props[p2], CollidaPoint);
                    }
                }
            }
        }

        /// <summary>
        /// Check if prop 1's points are within prop 2's frame, if so, resolve collision.
        /// </summary>
        public PolyPoint CheckPointCollision(Prop P1, Prop P2)
        {
            PolyPoint CollidaPoint = null;
            for(int p = 0; p < P1.Frame.Points.Count; p++)
            {
                if(P2.PointCollides(P1.Frame.Points[p]))
                {
                    CollidaPoint = P1.Frame.Points[p];
                    //ResolvePropPointCollision(P1, P2, P1.Frame.Points[p]);
                }
            }
            return CollidaPoint;
        }

        /// <summary>
        /// This function REWINDS TIME to move two props out of a collision state.
        /// </summary>
        public void PullPropsOutOfCollision(Prop PointProp, Prop EdgeProp)
        {
            PolyPoint CollidaPoint = null;
            int DepthCounter = 0;
            float DepthBase = .5f;
            float DepthWidth = .25f;
            Vector3 PointPosition = PointProp.Position - PointProp.Velocity;
            Vector3 EdgePosition = EdgeProp.Position - EdgeProp.Velocity;
            Vector3 PointRotation = PointProp.Rotation - PointProp.AngularVelocity;
            Vector3 EdgeRotation = EdgeProp.Rotation - EdgeProp.AngularVelocity;

            while (DepthCounter < 10)
            {
                //Probe forward half step and test for collision
                float InFactor = DepthBase + DepthWidth;
                PointProp.SetPosition(PointPosition + PointProp.Velocity * InFactor);
                PointProp.SetRotation(PointRotation + PointProp.AngularVelocity * InFactor);
                EdgeProp.SetPosition(EdgePosition + EdgeProp.Velocity * InFactor);
                EdgeProp.SetRotation(EdgeRotation + EdgeProp.AngularVelocity * InFactor);

                CollidaPoint = CheckPointCollision(PointProp, EdgeProp);

                //No collision, keep half step and set up next probe
                if (CollidaPoint == null)
                {
                    DepthBase += DepthWidth;
                }
                //Collision, half step back, set up next probe
                else
                {
                    float OutFactor = DepthBase - DepthWidth;
                    PointProp.SetPosition(PointPosition + PointProp.Velocity * OutFactor);
                    PointProp.SetRotation(PointRotation + PointProp.AngularVelocity * OutFactor);
                    EdgeProp.SetPosition(EdgePosition + EdgeProp.Velocity * OutFactor);
                    EdgeProp.SetRotation(EdgeRotation + EdgeProp.AngularVelocity * OutFactor);

                    DepthBase -= DepthWidth;
                }
                DepthWidth /= 2;

                DepthCounter++;
            }
        }

        public void SetPropsOutOfCollision(Prop PointProp, Prop EdgeProp, PolyPoint ImpactPoint)
        {
            Vector3 EdgeDistance = EdgeProp.ShortestPath(ImpactPoint);
            if (PointProp.Mass <= EdgeProp.Mass)
            {
                PointProp.Nudge(-EdgeDistance);
                ImpactPoint.Position -= EdgeDistance;
            }
            else
            {
                EdgeProp.Nudge(EdgeDistance);
            }
        }
        public void SetPropsUpOfCollision(Prop PointProp, Prop EdgeProp, PolyPoint ImpactPoint)
        {
            Vector3 EdgeDistance = EdgeProp.HighLinePath(new Line(ImpactPoint, new PolyPoint(ImpactPoint.Position + new Vector3(0, -1, 0))));
            if (PointProp.Mass <= EdgeProp.Mass)
            {
                PointProp.Nudge(EdgeDistance);
                ImpactPoint.Position += EdgeDistance;
            }
            else
            {
                EdgeProp.Nudge(-EdgeDistance);
            }
        }
        
        /// <summary>
        /// Applies an impulse to two props that have been shown to be in a collision state at a given impact point.
        /// </summary>
        public void ResolvePropPointCollision(Prop PointProp, Prop EdgeProp, PolyPoint ImpactPoint)
        {
            if (PointProp is ChainHead)
            {
                //ChainHead.Root();
                StretchChain.Stick(EdgeProp);
            }
            else if (PointProp is Player && EdgeProp.Name.Equals("SmallIsoTriangle.txt") || PointProp.Name.Equals("SmallIsoTriangle.txt") && EdgeProp is Player && !Player.Removing)
            {
                KillPlayer();
            }
            else if (PointProp is Player && EdgeProp.Name.Equals("SnapTrapSnap.txt") || PointProp.Name.Equals("SnapTrapSnap.txt") && EdgeProp is Player && !Player.Removing)
            {
                KillPlayer();
            }

            int IPN = -1;
            for(int p = 0;p < PointProp.Frame.Points.Count;p++)
            {
                if (PointProp.Frame.Points[p] == ImpactPoint)
                {
                    IPN = p;
                }
            }

            if (!(PointProp is Player) && !(EdgeProp is Player))
            {
                PullPropsOutOfCollision(PointProp, EdgeProp);
            }
            //PullPropsOutOfCollision(PointProp, EdgeProp);

            if(IPN != -1)
                ImpactPoint = PointProp.Frame.Points[IPN];

            if (PointProp is Player && IPN == 2)
                SetPropsUpOfCollision(PointProp, EdgeProp, ImpactPoint);
            else
                SetPropsOutOfCollision(PointProp, EdgeProp, ImpactPoint);

            Line ImpactSurface = EdgeProp.LineClosestToPoint(ImpactPoint);
            Vector3 NormalVector = new Vector3(ImpactSurface.Perpendicular().X, ImpactSurface.Perpendicular().Y, 0);
            NormalVector.Normalize();

            Vector3 RVectorA = ImpactPoint.Position - PointProp.Position;
            Vector3 RVectorPerpA = new Vector3(-RVectorA.Y, RVectorA.X, 0);
            Vector3 RVectorB = ImpactPoint.Position - EdgeProp.Position;
            Vector3 RVectorPerpB = new Vector3(-RVectorB.Y, RVectorB.X, 0);

            if (Math.Abs(RVectorA.Y) >= Math.Abs(RVectorA.X) && Math.Abs(ImpactSurface.YSlope) < 1.5)
            {
                PointProp.Grounded = true;
            }

            int t;  // checkin fo bugs
            if(PointProp is Player && !PointProp.Grounded)
                t = 0;

            Vector3 ImpactVelocityA = PointProp.Velocity + Vector3.Cross(PointProp.AngularVelocity, RVectorA);
            Vector3 ImpactVelocityB = EdgeProp.Velocity + Vector3.Cross(EdgeProp.AngularVelocity, RVectorB);
            Vector3 RelativeImpactVelocity = ImpactVelocityA - ImpactVelocityB;

            float Elasticity;
            if (PointProp is Player || EdgeProp is Player)
                Elasticity = 0;
            else
                Elasticity = ELASTICITY;
            float MassA = PointProp.Mass;
            float MassB = EdgeProp.Mass;
            float InertiaA = PointProp.Inertia;
            float InertiaB = EdgeProp.Inertia;

            float ImpulseNumerator = Vector3.Dot(-(1 + Elasticity) * RelativeImpactVelocity, NormalVector);
            float ImpulseMassPortion = Vector3.Dot(NormalVector, NormalVector) * (1 / MassA + 1 / MassB);
            float ImpulseAnglePortion = (float)Math.Pow(Vector3.Dot(RVectorPerpA, NormalVector), 2) / InertiaA + (float)Math.Pow(Vector3.Dot(RVectorPerpB, NormalVector), 2) / InertiaB;

            float Impulse = ImpulseNumerator / (ImpulseMassPortion + ImpulseAnglePortion);

            Vector3 PointCollisionForce = Impulse * NormalVector;
            float PointCollisionTorque = Vector3.Dot(RVectorPerpA, Impulse * NormalVector);
            Vector3 EdgeCollisionForce = -Impulse * NormalVector;
            float EdgeCollisionTorque = -Vector3.Dot(RVectorPerpB, Impulse * NormalVector);

            PointProp.QueueForce(PointCollisionForce);
            PointProp.QueueTorque(new Vector3(0, 0, PointCollisionTorque));
            EdgeProp.QueueForce(EdgeCollisionForce);
            EdgeProp.QueueTorque(new Vector3(0, 0, EdgeCollisionTorque));
        }

        public void ApplyPropGravity()
        {
            foreach (Prop P in Props)
            {
                if (P.GravityOn)
                    P.QueueForce(P.Mass * GRAVITY);
            }
        }

        public void UnGroundProps()
        {
            foreach (Prop P in Props)
                P.Grounded = false;
        }

        public void AddHinge(Prop P1, Vector3 Hinge1, Prop P2, Vector3 Hinge2)
        {
            P1.AddHingePoint(Hinge1, P2, Hinge2);
            HingeLinks.Add(P1.HingeLinks[P1.HingeLinks.Count - 1]);
        }
        public void AddHingeToChain(HingeChain HC, Prop P1, Vector3 Hinge1, Prop P2, Vector3 Hinge2)
        {
            P1.AddHingePoint(Hinge1, P2, Hinge2);
            HC.HingeLinks.Add(P1.HingeLinks[P1.HingeLinks.Count - 1]);
        }

        public void EnforceHingeConstraints(HingeChain HC)
        {
            int Size = HC.HingeLinks.Count;
            Matrix3x3[,] ConstraintMatrix = new Matrix3x3[Size, Size];
            Vector3[] ConstraintVector = new Vector3[Size];
            Vector3[] ForceVector = new Vector3[Size];
            Prop LeftProp, RightProp;
            Vector3 OuterLeftHingeRadius, InnerLeftHingeRadius, InnerRightHingeRadius, OuterRightHingeRadius;
            Matrix3x3 OuterLeftHingeMatrix, InnerLeftHingeMatrix, InnerRightHingeMatrix, OuterRightHingeMatrix;
            Vector3 InnerLeftHingeVector, InnerRightHingeVector;

            //Populate constraint matrix and vector
            for (int r = 0; r < Size; r++)
            {
                for (int c = 0; c < Size; c++)
                {
                    if (r == c)
                    {
                        LeftProp = HC.HingeLinks[r].Prop1;
                        InnerLeftHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r].HingeOffset1, LeftProp.Rotation.Z);
                        InnerLeftHingeMatrix = CalculateHingeMatrix(LeftProp, InnerLeftHingeRadius);
                        RightProp = HC.HingeLinks[r].Prop2;
                        InnerRightHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r].HingeOffset2, RightProp.Rotation.Z);
                        InnerRightHingeMatrix = CalculateHingeMatrix(RightProp, InnerRightHingeRadius);

                        ConstraintMatrix[r, c] = InnerLeftHingeMatrix.Add(InnerRightHingeMatrix);
                    }
                    else if (r + 1 == c)
                    {
                        RightProp = HC.HingeLinks[r + 1].Prop1;
                        OuterRightHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r + 1].HingeOffset1, RightProp.Rotation.Z);
                        OuterRightHingeMatrix = CalculateHingeMatrix(RightProp, OuterRightHingeRadius);

                        ConstraintMatrix[r, c] = OuterRightHingeMatrix.Multiply(-1);
                    }
                    else if (r - 1 == c)
                    {
                        LeftProp = HC.HingeLinks[r - 1].Prop2;
                        OuterLeftHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r - 1].HingeOffset2, LeftProp.Rotation.Z);
                        OuterLeftHingeMatrix = CalculateHingeMatrix(LeftProp, OuterLeftHingeRadius);

                        ConstraintMatrix[r, c] = OuterLeftHingeMatrix.Multiply(-1);
                    }
                    else
                    {
                        ConstraintMatrix[r, c] = new Matrix3x3();
                    }
                }

                LeftProp = HC.HingeLinks[r].Prop1;
                InnerLeftHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r].HingeOffset1, LeftProp.Rotation.Z);
                InnerLeftHingeVector = CalculateHingeVector(LeftProp, InnerLeftHingeRadius);
                RightProp = HC.HingeLinks[r].Prop2;
                InnerRightHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r].HingeOffset2, RightProp.Rotation.Z);
                InnerRightHingeVector = CalculateHingeVector(RightProp, InnerRightHingeRadius);

                ConstraintVector[r] = InnerRightHingeVector - InnerLeftHingeVector;
            }

            ReduceLowerRows(ConstraintMatrix, ConstraintVector, Size);
            ReduceUpperRows(ConstraintMatrix, ConstraintVector, Size);
            
            //Apply forces determined from reduced constraint matrix
            for (int r = 0; r < Size; r++)
            {
                Matrix3x3 InverseMatrixA = ConstraintMatrix[r, r].Inverse();
                ForceVector[r] = InverseMatrixA.Multiply(ConstraintVector[r]);

                Vector3 LeftHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r].HingeOffset1, HC.HingeLinks[r].Prop1.Rotation.Z);
                Vector3 RightHingeRadius = Prop.RotateVectorAboutZ(HC.HingeLinks[r].HingeOffset2, HC.HingeLinks[r].Prop2.Rotation.Z);

                HC.HingeLinks[r].Prop1.QueueForceWithOffset(ForceVector[r], LeftHingeRadius);
                HC.HingeLinks[r].Prop2.QueueForceWithOffset(-ForceVector[r], RightHingeRadius);
            }
            //HC.SetHingePositions();
            //HC.SpringHingePositions();
        }

        public void ReduceLowerRows(Matrix3x3[,] ConstraintMatrix, Vector3[] ConstraintVector, int Size)
        {
            for (int r = 0; r < Size - 1; r++)
            {
                Matrix3x3 InversePivotMatrix = ConstraintMatrix[r, r].Inverse();
                Matrix3x3 NegativeTargetMatrix = ConstraintMatrix[r + 1, r].Multiply(-1);
                Matrix3x3 TransformMatrix = NegativeTargetMatrix.Multiply(InversePivotMatrix);

                for (int c = 0; c < Size; c++)
                {
                    ConstraintMatrix[r + 1, c] = ConstraintMatrix[r + 1, c].Add(TransformMatrix.Multiply(ConstraintMatrix[r, c]));
                }
                ConstraintVector[r + 1] += TransformMatrix.Multiply(ConstraintVector[r]);
            }
        }
        public void ReduceUpperRows(Matrix3x3[,] ConstraintMatrix, Vector3[] ConstraintVector, int Size)
        {
            for (int r = Size - 1; r > 0; r--)
            {
                Matrix3x3 InversePivotMatrix = ConstraintMatrix[r, r].Inverse();
                Matrix3x3 NegativeTargetMatrix = ConstraintMatrix[r - 1, r].Multiply(-1);
                Matrix3x3 TransformMatrix = NegativeTargetMatrix.Multiply(InversePivotMatrix);

                for (int c = 0; c < Size; c++)
                {
                    ConstraintMatrix[r - 1, c] = ConstraintMatrix[r - 1, c].Add(TransformMatrix.Multiply(ConstraintMatrix[r, c]));
                }
                ConstraintVector[r - 1] += TransformMatrix.Multiply(ConstraintVector[r]);
            }
        }

        public Matrix3x3 CalculateHingeMatrix(Prop P, Vector3 HingeRadius)
        {
            Matrix3x3 MassMatrix = new Matrix3x3(P.Mass);
            MassMatrix = MassMatrix.SimpleInverse();
            Matrix3x3 InertiaMatrix = new Matrix3x3(P.Inertia);
            InertiaMatrix = InertiaMatrix.SimpleInverse();

            Matrix3x3 TildeHingeRadius = new Matrix3x3(HingeRadius);

            Matrix3x3 ConstantMatrix = TildeHingeRadius.Multiply(InertiaMatrix);
            ConstantMatrix = ConstantMatrix.Multiply(TildeHingeRadius);

            ConstantMatrix = MassMatrix.Subtract(ConstantMatrix);
            return ConstantMatrix;
        }

        public Vector3 CalculateHingeVector(Prop P, Vector3 HingeRadius)
        {
            Matrix3x3 MassMatrix = new Matrix3x3(P.Mass);
            MassMatrix = MassMatrix.SimpleInverse();
            Matrix3x3 InertiaMatrix = new Matrix3x3(P.Inertia);
            Matrix3x3 InverseInertiaMatrix = InertiaMatrix.SimpleInverse();

            Vector3 ForceVector = MassMatrix.Multiply(P.QueuedForce);
            Vector3 AngleVector = Vector3.Cross(P.AngularVelocity, Vector3.Cross(P.AngularVelocity, HingeRadius));

            Vector3 PointVelocity = Vector3.Cross(P.AngularVelocity, HingeRadius);
            Vector3 AngularMomentum = InertiaMatrix.Multiply(P.AngularVelocity);
            Vector3 TorqueVector = InverseInertiaMatrix.Multiply(P.QueuedTorque);
            Vector3 InertiaMomentum = InverseInertiaMatrix.Multiply(Vector3.Cross(P.AngularVelocity, AngularMomentum));

            Vector3 HingeVector = ForceVector + AngleVector + Vector3.Cross(TorqueVector - InertiaMomentum, HingeRadius);
            return HingeVector;
        }

        /// <summary>
        /// Start a collision check for each prop and subprop in the environment.
        /// </summary>
        /*public void CheckPropCollision(List<Prop> PList)
        {
            for(int p = 0; p < PList.Count; p++)
            {
                if(PList[p].AttachedProps.Count != 0)
                {
                    CheckPropCollision(PList[p].AttachedProps);
                }
                CheckAgainstProps(PList[p], Props);
            }
        }*/

        /// <summary>
        /// Compare each given prop against every other prop and subprop, if they share a layer, check point collision.
        /// </summary>
        /*public void CheckAgainstProps(Prop P, List<Prop> PList)
        {
            for(int p = 0; p < PList.Count; p++)
            {
                if(PList[p].AttachedProps.Count != 0)
                {
                    CheckAgainstProps(P, PList[p].AttachedProps);
                }
                if(P != PList[p] && Prop.HasCommonLayer(P, PList[p]))
                {
                    PolyPoint CollidaPoint = CheckPointCollision(P, PList[p]);
                    if (CollidaPoint != null)
                        ResolvePropPointCollision(P, PList[p], CollidaPoint);
                }
            }
        }*/

        /*public void HandleHingePull()
        {
            Vector2 P1 = Props[0].Frame.Points[0].Position;
            Vector2 P2 = Props[1].Frame.Points[2].Position;
            Vector2 PDistance = P2 - P1;

            float MassFactor1 = Props[0].Mass / (Props[0].Mass + Props[1].Mass);
            float MassFactor2 = Props[1].Mass / (Props[0].Mass + Props[1].Mass);

            Props[0].Position += MassFactor1 * PDistance;
            Props[1].Position -= MassFactor2 * PDistance;
        }*/

        /*public void CheckPointCollision()
        {
            //ResolveBothLoose();

            //ResolveFirstLoose();
            //ResolveSecondLoose();

            for(int a = 0; a < Props.Count; a++)
            {
                for(int b = 0; b < Props.Count; b++)
                {
                    if(a != b && HasCommonLayer(Props[a], Props[b]))
                    {
                        for(int p = 0; p < Props[a].Frame.Points.Count; p++)
                        {
                            if(Props[b].PointCollides(Props[a].Frame.Points[p]))
                            {
                                ResolvePropPointCollision(Props[a], Props[b], Props[a].Frame.Points[p]);
                            }
                        }
                    }
                }
            }
        }*/

        /*public void ResolveFirstLoose()
        {
            for(int p = 0; p < Props[0].Frame.Points.Count; p++)
            {
                TestBool = Props[1].PointCollides(Props[0].Frame.Points[p]);
                if(TestBool)
                {
                    PolyPoint ImpactPoint = Props[0].Frame.Points[p];
                    Vector2 NudgeDistance = Props[1].ShortestPath(ImpactPoint);
                    Props[0].Nudge(NudgeDistance);
                    ImpactPoint.Position += NudgeDistance;

                    Line ImpactSurface = Props[1].LineClosestToPoint(ImpactPoint);
                    Vector2 NormalVector = ImpactSurface.Perpendicular();
                    NormalVector.Normalize();

                    Vector2 RVectorA = ImpactPoint.Position - Props[0].Frame.Origin.Position;
                    Vector2 RVectorPerpA = new Vector2(-RVectorA.Y, RVectorA.X);
                    Vector2 RVectorB = ImpactPoint.Position - Props[1].Frame.Origin.Position;
                    Vector2 RVectorPerpB = new Vector2(-RVectorB.Y, RVectorB.X);

                    Vector2 ImpactVelocityA = Props[0].Velocity + Props[0].AngularVelocity * RVectorPerpA;
                    Vector2 ImpactVelocityB = Props[1].Velocity + Props[1].AngularVelocity * RVectorPerpB;
                    Vector2 RelativeImpactVelocity = ImpactVelocityA - ImpactVelocityB;

                    float Elasticity = 1;
                    float MassA = 1;
                    float MassB = float.PositiveInfinity;
                    float InertiaA = 1000;
                    float InertiaB = float.PositiveInfinity;
                    
                    float ImpulseNumerator = Vector2.Dot(-(1 + Elasticity) * RelativeImpactVelocity, NormalVector);
                    float ImpulseMassPortion = Vector2.Dot(NormalVector, NormalVector) * (1 / MassA + 1 / MassB);
                    float ImpulseAnglePortion = (float)Math.Pow(Vector2.Dot(RVectorPerpA, NormalVector), 2) / InertiaA + (float)Math.Pow(Vector2.Dot(RVectorPerpB, NormalVector), 2) / InertiaB;

                    float Impulse = ImpulseNumerator / (ImpulseMassPortion + ImpulseAnglePortion);

                    Props[0].Velocity += (Impulse / MassA) * NormalVector;
                    Props[0].AngularVelocity += Vector2.Dot(RVectorPerpA, Impulse * NormalVector) / InertiaA;
                    Props[1].Velocity -= (Impulse / MassB) * NormalVector;
                    Props[1].AngularVelocity -= Vector2.Dot(RVectorPerpB, Impulse * NormalVector) / InertiaB;
                }
            }
        }

        public void ResolveSecondLoose()
        {
            for(int p = 0; p < Props[1].Frame.Points.Count; p++)
            {
                TestBool = Props[0].PointCollides(Props[1].Frame.Points[p]);
                if(TestBool)
                {
                    PolyPoint ImpactPoint = Props[1].Frame.Points[p];
                    Vector2 NudgeDistance = Props[0].ShortestPath(ImpactPoint);
                    NudgeDistance *= -1;
                    Props[0].Nudge(NudgeDistance);

                    Line ImpactSurface = Props[0].LineClosestToPoint(ImpactPoint);
                    Vector2 NormalVector = ImpactSurface.Perpendicular();
                    NormalVector.Normalize();

                    Vector2 RVectorA = ImpactPoint.Position - Props[1].Frame.Origin.Position;
                    Vector2 RVectorPerpA = new Vector2(-RVectorA.Y, RVectorA.X);
                    Vector2 RVectorB = ImpactPoint.Position - Props[0].Frame.Origin.Position;
                    Vector2 RVectorPerpB = new Vector2(-RVectorB.Y, RVectorB.X);

                    Vector2 ImpactVelocityA = Props[1].Velocity + Props[1].AngularVelocity * RVectorPerpA;
                    Vector2 ImpactVelocityB = Props[0].Velocity + Props[0].AngularVelocity * RVectorPerpB;
                    Vector2 RelativeImpactVelocity = ImpactVelocityA - ImpactVelocityB;

                    float Elasticity = 1;
                    float MassB = 1;
                    float InertiaB = 1000;

                    float ImpulseNumerator = Vector2.Dot(-(1 + Elasticity) * RelativeImpactVelocity, NormalVector);
                    float ImpulseMassPortion = Vector2.Dot(NormalVector, NormalVector) * (1 / MassB);
                    float ImpulseAnglePortion = (float)Math.Pow(Vector2.Dot(RVectorPerpB, NormalVector), 2) / InertiaB;
                    float Impulse = ImpulseNumerator / (ImpulseMassPortion + ImpulseAnglePortion);

                    Props[0].Velocity -= (Impulse / MassB) * NormalVector;
                    Props[0].AngularVelocity -= Vector2.Dot(RVectorPerpB, Impulse * NormalVector) / InertiaB;
                }
            }
        }

        public void ResolveBothLoose()
        {
            for(int p = 0; p < Props[0].Frame.Points.Count; p++)
            {
                TestBool = Props[1].PointCollides(Props[0].Frame.Points[p]);
                if(TestBool)
                {
                    PolyPoint ImpactPoint = Props[0].Frame.Points[p];
                    Props[0].Position += Props[1].ShortestPath(ImpactPoint);

                    Line ImpactSurface = Props[1].LineClosestToPoint(ImpactPoint);
                    Vector2 NormalVector = ImpactSurface.Perpendicular();
                    NormalVector.Normalize();

                    Vector2 RVectorA = ImpactPoint.Position - Props[0].Frame.Origin.Position;
                    Vector2 RVectorPerpA = new Vector2(-RVectorA.Y, RVectorA.X);
                    Vector2 RVectorB = ImpactPoint.Position - Props[1].Frame.Origin.Position;
                    Vector2 RVectorPerpB = new Vector2(-RVectorB.Y, RVectorB.X);

                    Vector2 ImpactVelocityA = Projection(NormalVector, Props[0].Velocity);
                    Vector2 ImpactVelocityB = Projection(NormalVector, Props[1].Velocity);
                    Vector2 RelativeImpactVelocity = ImpactVelocityA - ImpactVelocityB;

                    float Elasticity = 1;
                    float MassA = 10;
                    float MassB = 10;
                    float InertiaA = 100;
                    float InertiaB = 100;

                    float ImpulseNumerator = Vector2.Dot(-(1 + Elasticity) * RelativeImpactVelocity, NormalVector);
                    float ImpulseMassPortion = Vector2.Dot(NormalVector, NormalVector) * (1 / MassA + 1 / MassB);
                    float ImpulseAnglePortion = (float)Math.Pow(Vector2.Dot(RVectorPerpA, NormalVector), 2) / InertiaA + (float)Math.Pow(Vector2.Dot(RVectorPerpB, NormalVector), 2) / InertiaB;
                    float Impulse = ImpulseNumerator / (ImpulseMassPortion + ImpulseAnglePortion);

                    Props[0].Velocity += (Impulse / MassA) * NormalVector;
                    Props[0].AngularVelocity += Vector2.Dot(RVectorPerpA, Impulse * NormalVector) / InertiaA;
                    Props[1].Velocity -= (Impulse / MassB) * NormalVector;
                    Props[1].AngularVelocity -= Vector2.Dot(RVectorPerpB, Impulse * NormalVector) / InertiaB;
                }
            }
        }*/

        /*public void ResolveIntersectionCollision()
        {
            CollisionList.Clear();

            List<PolyPoint> PointList = (Props[0].Frame.CollidesWith(GroundLine));
            foreach(PolyPoint p in PointList)
            {
                bool PointDuplicate = false;
                foreach(PolyPoint CP in CollisionList)
                {
                    if(p.Position == CP.Position)
                    {
                        PointDuplicate = true;
                    }
                }
                if(!PointDuplicate)
                {
                    CollisionList.Add(p);
                }
            }

            Line CLine1, CLine2;
            CLine1 = CLine2 = null;
            for(int p = 0; p < CollisionList.Count; p++)
            {
                bool LineFound = false;
                for(int l = 0; l < Props[0].Frame.OuterLines.Count; l++)
                {
                    if(Props[0].Frame.OuterLines[l].ContainsPoint(CollisionList[p]))
                    {
                        if(CLine1 == null)
                        {
                            CLine1 = Props[0].Frame.OuterLines[l];
                            LineFound = true;
                        }
                        else if(CLine2 == null)
                        {
                            CLine2 = Props[0].Frame.OuterLines[l];
                            LineFound = true;
                        }
                    }
                    if(LineFound)
                    {
                        break;
                    }
                }
            }
            PolyPoint PTip, PImp;
            PTip = PImp = new PolyPoint(Vector2.Zero);
            bool TipSet, ImpSet;
            TipSet = ImpSet = false;
            if(CLine1 != null && CLine2 != null)
            {
                PTip = CommonPoint(CLine1, CLine2);
                if(PTip != null)
                    TipSet = true;
            }
            if(CollisionList.Count > 1)
            {
                PImp = PolyPoint.AveragePoint(CollisionList[0], CollisionList[1]);
                ImpSet = true;
            }
            if(TipSet && ImpSet)
            {
                Props[0].Position += PImp.Position - PTip.Position;
            }

            CollisionList.AddRange(Props[0].Frame.CollidesWith(Props[1].Frame));
        }*/
    }
}
